/**
 * @file vision.c
 * @brief 视觉模块通信与控制实现文件，负责与视觉处理模块的数据收发及目标角度解算
 * @author WeiFan Qiu (qiuweifan0523@faxmail.com)
 * @version 1.0
 * @date 2025-08-08
 * 
 * @copyright Copyright (c) 2025  邱伟凡
 * 
 * @par 修改日志:
 * @bug 待解决问题:
 */
// 包含依赖头文件
// 包含依赖头文件
#include "vision.h"          // 视觉模块接口定义
#include "head.h"            // 云台主控制头文件
#include "communicate.h"     // 通信协议头文件
#include "freertos.h"        // FreeRTOS内核
#include "task.h"            // FreeRTOS任务管理
#include "arm_math.h"        // ARM数学库（用于角度计算）
#include "usbd_cdc_if.h"     // USB虚拟串口收发接口
#include "state.h"           // 系统状态管理
#include "QWF_Math.h"        // 自定义数学工具库
#include "ctrl.h"			 //控制器相关

uint8_t RX_Data_buffer[16];
static vision_t Vision={
.tx={.header=0xFF,.tail=0x0D,.mode=0},
.rx.use_fire_advice=1,
};
vision_t* vision=&Vision;
/**
 * @brief 视觉数据发送任务（FreeRTOS任务）
 * @param argument 任务参数（未使用）
 * @details 周期性（3ms）调用Vision_Send发送数据，确保视觉模块实时获取云台状态的同时视觉不会卡顿
 */
void Vision_Send(void){
	vision->tx.pitch=pitch_angle_meas;
	vision->tx.yaw=yaw_angle_meas;
	vision->tx.roll= 0;
	
//	vision->tx.roll= dm_imu->euler[2];
	vision->tx.roll= 0;	
	vision->tx.check_byte    = 0;   //(s)game time[o,450]  
	CDC_Transmit_HS((uint8_t *)&vision->tx, 16);
}
void vision_task(void const * argument){
	static TickType_t xLastWakeTime;
    const TickType_t xPeriod = pdMS_TO_TICKS(3);// 周期太小视觉会卡
    // 初始化唤醒时间
    xLastWakeTime = xTaskGetTickCount();
	while(1)
	{
		Vision_Send();	
		 vTaskDelayUntil(&xLastWakeTime, xPeriod);	
	}
}
/**
 * @brief 接收并解析视觉模块发送的数据
 * @details 通过USB虚拟串口接收数据，校验头尾标识后提取有效信息（目标角度、射击建议等）
 */
void Vision_Receive(void){  
	USBD_Interface_fops_HS.Receive(RX_Data_buffer,(uint32_t *)16);
	/*帧头帧尾识别通过
	vision->rx.header=RX_Data_buffer[0];
	vision->rx.tail=RX_Data_buffer[15];
	*/
	if(RX_Data_buffer[0]==vision_rx_header&&RX_Data_buffer[15]==vision_rx_tail){
		vision->rx.distance = *(float*)(RX_Data_buffer+10);
		communicate->head.vision.data.str.head_ok=1;	
		qwf_state->head.vision.online.tick=qwf_state->sys_tick;
		if( vision->rx.distance>0){//认为是有效数据
			memcpy(&vision->rx, RX_Data_buffer, 16);
		/*
			vision->rx.fire_advice = RX_Data_buffer[1];	
			vision->rx.pitch = *(float*)(RX_Data_buffer+2);
	
			vision->rx.yaw = *(float*)(RX_Data_buffer+6);
	
			vision->rx.check_byte = RX_Data_buffer[14];
		*/
		}

	}
//	else
//		((Gimbal_Mode_Communicate_Struct_Type*)(communicate_data[Gimbal_Mode_Data]->Data))->vision_head_ok=0;	
}
/*-----------------偏置-------------------*/
float yaw_add=0;//		旋转		0定
float pitch_add=0;//					0.5
/**
 * @brief 视觉目标角度解算与控制
 * @details 根据视觉接收的目标角度计算云台目标位置，并结合控制模式（键鼠/遥控器）调整
 */
void Vision_Control(void){	
	yaw_angle_target=find_min_angle(vision->rx.yaw+yaw_add,yaw_angle_meas_gyro,360);
	pitch_angle_target=find_min_angle(vision->rx.pitch+pitch_add,pitch_angle_meas_gyro,360);		
	if( ctrl->mode==KM){
		yaw_angle_target-=ctrl->KM[now].mouse.x*KM_Yaw_factor*10;
		pitch_angle_target=QWF_Limiter(pitch_angle_target-ctrl->KM[now].mouse.y*KM_Pitch_factor*-10,\
						PITCH_MAX,PITCH_MIN);				
	}
	
}
